﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using Microsoft.Kinect;

namespace DTW_Beta
{
    class Skeleton2DDataExtract
    {
        // Skeleton2DdataCoordEventHandler delegate
        public delegate void Skeleton2DDataCoordEventHandler(object sender, Skeleton2DDataCoordEventArgs a);

        // The Skeleton 2Ddata Coord Ready event
        public static event Skeleton2DDataCoordEventHandler Skeleton2DdataCoordReady;

        // Crunches Kinect SDK's Skeleton Data and spits out a format more useful for DTW
        public static void ProcessData(Skeleton data)
        {
            // Extract the coordinates of the points.
            var p = new Point[6];
            Point shoulderRight = new Point(), shoulderLeft = new Point();
            if (data != null)
            {
                foreach (Joint j in data.Joints)
                {
                    switch (j.JointType)
                    {
                        case JointType.HandLeft:
                            p[0] = new Point(j.Position.X, j.Position.Y);
                            break;
                        case JointType.WristLeft:
                            p[1] = new Point(j.Position.X, j.Position.Y);
                            break;
                        case JointType.ElbowLeft:
                            p[2] = new Point(j.Position.X, j.Position.Y);
                            break;
                        case JointType.ElbowRight:
                            p[3] = new Point(j.Position.X, j.Position.Y);
                            break;
                        case JointType.WristRight:
                            p[4] = new Point(j.Position.X, j.Position.Y);
                            break;
                        case JointType.HandRight:
                            p[5] = new Point(j.Position.X, j.Position.Y);
                            break;
                        case JointType.ShoulderLeft:
                            shoulderLeft = new Point(j.Position.X, j.Position.Y);
                            break;
                        case JointType.ShoulderRight:
                            shoulderRight = new Point(j.Position.X, j.Position.Y);
                            break;
                    }
                }

                // Centre the data
                var center = new Point((shoulderLeft.X + shoulderRight.X) / 2, (shoulderLeft.Y + shoulderRight.Y) / 2);
                for (int i = 0; i < 6; i++)
                {
                    p[i].X -= center.X;
                    p[i].Y -= center.Y;
                }
                
                // Normalization of the coordinates
                double shoulderDist = 
                    Math.Sqrt(Math.Pow((shoulderLeft.X - shoulderRight.X), 2) +
                              Math.Pow((shoulderLeft.Y - shoulderRight.Y), 2));

                for (int i = 0; i < 6; i++)
                {
                    p[i].X /= shoulderDist;
                    p[i].Y /= shoulderDist;
                }

                // Launch the event!
                Skeleton2DdataCoordReady(null, new Skeleton2DDataCoordEventArgs(p));
            }
        }
    }
    
}
